#include <stdio.h>
#include <unistd.h>

#include "ohos_init.h"
#include "cmsis_os2.h"
#include "iot_gpio.h"
#include "iot_gpio_ex.h"
#include "iot_watchdog.h"
#include "iot_pwm.h"
#include <stdarg.h>

#define x_L_cm      16
#define x_half_L_cm 8
#define speed2duty(x)  (2.03 * x)

typedef enum enginer_property
{
    ENGINER_STOP  = 0x00010001,
    ENGINER_FREE  = 0x00000000,
    ENGINER_AHEAD = 0x00010000,
    ENGINER_BACK  = 0x00000001
}enginer_property_t;


typedef struct enginer_direction_control_pin
{
    uint32_t first_id;
    uint32_t second_id;

}enginer_direction_control_pin_t;

typedef struct pwm_controller
{
    uint16_t duty;
    uint32_t id;
    uint32_t port;
}pwm_controller_t;


typedef struct enginer
{
    pwm_controller_t                pwm;
    enginer_direction_control_pin_t pin_group;
    enginer_property_t              property;
}enginer_device_t;

typedef struct car_controller
{
    enginer_device_t *x_negative;
    enginer_device_t *x_positive;                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                
}car_controller_t;

typedef enum
{
    MOTION_METHOD_STRAIGHT,
    MOTION_METHOD_CYCLE,
    MOTION_METHOD_STOP
}motion_method_t;

car_controller_t car;


void motion_stright(const car_controller_t *controller, float speed);
void motion_cycle(const car_controller_t *controller, float speed, float r);

//***********************************************
//    x_negative(-)   ^    x-positive(+)       **
//                    |<---------r+-------->|  **   
//         ***        |        ***          |  **
//      ---***-----------------***----------.  **
//         ***<-------L------->***             **
//          |                   |              **
//          -->>>>>>wheel<<<<<<--              **
//                                             **
//***********************************************

void enginer_setProperty(enginer_device_t *device, enginer_property_t property, uint16_t duty)
{
    device->property            = property;
    device->pwm.duty            = duty;
}

uint32_t car_motion(const car_controller_t *controller, motion_method_t method, float speed, ...)
{
    float r;
    va_list valist;
    switch (method)
    {
    case MOTION_METHOD_STRAIGHT: motion_stright(controller,speed);
        break;
    case MOTION_METHOD_STOP    : 
        controller->x_negative->property = ENGINER_STOP;
        controller->x_positive->property = ENGINER_STOP;
        break;
    default:
        va_start(valist,1);
        r = va_arg(valist,float);
        va_end(valist);
        break;
    }

    return 0;
}

void motion_stright(const car_controller_t *controller, float speed)
{
    enginer_property_t negative,positive;
    if(speed = 0)
    {
        negative = ENGINER_FREE;
        positive = ENGINER_FREE;
    }
    else if(speed < 0)
    {
        negative = ENGINER_BACK;
        positive= ENGINER_BACK;
        speed = 0 - speed;
    }
    else
    {
        negative = ENGINER_AHEAD;
        positive = ENGINER_AHEAD;
    }
    enginer_setProperty(controller->x_negative, negative, speed2duty(spped));
    enginer_setProperty(controller->x_positive,  positive, speed2duty(spped));
}

void motion_cycle(const car_controller_t *controller, float speed, float r)
{
    float v_negative, v_positive;
    enginer_property_t negative,positive;
    if(speed = 0 || r = 0)
    {
        negative = ENGINER_FREE;
        positive = ENGINER_FREE;
        v_negative = 0;
        v_positive = 0;
    }
    else 
    {
        v_negative = speed*(1+x_half_L_cm/r);
        v_positive = speed*(1-x_half_L_cm/r);
        if(v_negative < 0)
        {
            negative = ENGINER_BACK;
            v_negative = 0 - v_negative;
        }
        else
        {
            negative = ENGINER_AHEAD;
        }
        if(v_positive < 0)
        {
            positive = ENGINER_BACK;
            v_positive = 0 - v_positive;
        }
        else
        {
            positive = ENGINER_AHEAD;
        }
    }
    enginer_setProperty(controller->x_negative, negative, speed2duty(v_negative));
    enginer_setProperty(controller->x_positive, positive, speed2duty(v_positive));
}

void *car_performer(void)
{
    while(1)
    {
        enginer_device_t *device;
        device = car.x_negative;
        IoTGpioSetOutputVal(device->pin_group.first_id, *(uint16_t*)(&(device->property)));
        IoTGpioSetOutputVal(device->pin_group.second_id, *((uint16_t*)(&(device->property)) + 1));
        IoTPwmStart(device->pwm.id,device->pwm.duty,1000);
        device = car.x_positive;
        IoTGpioSetOutputVal(device->pin_group.first_id, *(uint16_t*)(&(device->property)));
        IoTGpioSetOutputVal(device->pin_group.second_id, *((uint16_t*)(&(device->property)) + 1));
        IoTPwmStart(device->pwm.id,device->pwm.duty,1000);
    }
}

void start_carControl_Task(void)
{
    osThreadAttr_t attr=
    {
        .name = "car_control_task",
        .attr_bits = 0U,
        .cb_mem = NULL,
        .cb_size = 0U,
        .stack_mem = NULL,
        .stack_size = 1024,
        .priority   = osPriorityNormal,
    };
    IoTGpioSetOutputVal(car.x_negative->pin_group.first_id,0);
    IoTGpioSetOutputVal(car.x_negative->pin_group.second_id,0);
    IoTGpioSetOutputVal(car.x_positive->pin_group.first_id,0);
    IoTGpioSetOutputVal(car.x_positive->pin_group.second_id,0);
    car.x_negative->pwm.id = 2;
    car.x_positive->pwm.id = 3;
    IoTGpioSetOutputVal(car.x_negative->pwm.id,0);
    IoTGpioSetOutputVal(car.x_positive->pwm.id,0);

    IoSetFunc(car.x_negative->pin_group.first_id,0);
    IoSetFunc(car.x_negative->pin_group.second_id,0);
    IoSetFunc(car.x_positive->pin_group.first_id,0);
    IoSetFunc(car.x_positive->pin_group.second_id,0);
    IoTGpioSetDir(car.x_negative->pin_group.first_id, IOT_GPIO_DIR_OUT);
    IoTGpioSetDir(car.x_negative->pin_group.second_id, IOT_GPIO_DIR_OUT);
    IoTGpioSetDir(car.x_positive->pin_group.first_id, IOT_GPIO_DIR_OUT);
    IoTGpioSetDir(car.x_positive->pin_group.second_id, IOT_GPIO_DIR_OUT);

    IoSetFunc(car.x_negative->pwm.id,5);
    IoSetFunc(car.x_positive->pwm.id,5);
    IoTPwmStart(car.x_negative->pwm.id,20,1000);

    
    if(osThreadNew((osThreadFunc_t)car_performer, NULL, &attr) == NULL)
    {
        printf("Task \"%s\" faild to start\n",attr.name);
    }
}